Add calibrate with jacobians for Cal3Bundler #539
Conversation
varunagrawal
added
help wanted
|
Okay so this is breaking because calling code is getting confused which version of @dellaert should I merge the two |
Yes :-) |
|
@dellaert great call on the Implicit Function theorem. Thinking in math was the right approach 😄 |
varunagrawal
force-pushed
the
fix/cal3bundler-jacobians
branch
from
f2f98af to
8634d3f
Compare
| CHECK(assert_equal(numerical2,K.D2d_intrinsic(p),1e-7)); | ||
| Matrix Dcombined(2,5); | ||
| Dcombined << Dp, Dcal; | ||
| CHECK(assert_equal(Dcombined,K.D2d_intrinsic_calibration(p),1e-7)); |
There was a problem hiding this comment.
These methods are deprecated, which is why I removed these tests.
gtsam/geometry/Cal3Bundler.cpp
Outdated
| // df/pi = -I (pn and pi are independent args) | ||
| // Dcal = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn) | ||
| // Dp = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K | ||
| Matrix H_uncal_K, H_uncal_pn; |
There was a problem hiding this comment.
These matrices should be fixed-size. Also, cache H_uncal_pn here? (In a fixed size Matrix) finally, you can avoid calculating H_uncal_K using `Dcal ? &H_uncal_K : nullptr in first argument below.
There was a problem hiding this comment.
Quick question about caching H_uncal_pn: should I be calling uncalibrate with H_uncal_pn always?
|
Approved as I’ll trust you’ll make those final changes |
Added an additional
calibratemethod forCal3Bundlerwhich compute jacobians.Since the normal calibration method is iterative, I have approximated the jacobian as if a single iteration step has occurred, which should suffice assuming the projection is close to the ideal measurements.
I would definitely like a thorough review on this though.
Fixes #237